Learning Objectives

  1. Explain the concept of sensor fusion algorithm in robotics applications.
  2. Apply the complementary filter algorithm to fuse compass sensor and wheel encoder data for heading control of a mobile robot.
  3. Describe the limitation of complimentary filter

Recap

  • we have derived the odometry formula for wheeled mobile robot (differential-drive) with encoder input in the previous lecture class.
  • The encoder (with pulses input) odometry formula:
  • Modify to encoder (with radian input) for Webots simulator
  • We have implemented square path movement with the formula above in Webots simulator in the previous tutorial.
# === Odometry Update Function ===
def update_odometry():
    global prev_left, prev_right, pose

    cur_left = left_sensor.getValue()
    cur_right = right_sensor.getValue()

    dphi_l = cur_left - prev_left
    dphi_r = cur_right - prev_right
    prev_left = cur_left
    prev_right = cur_right

    delta_s = WHEEL_RADIUS * (dphi_r + dphi_l) / 2.0
    delta_theta = WHEEL_RADIUS * (dphi_r - dphi_l) / WHEEL_BASE
    theta_mid = pose[2] + delta_theta / 2.0

    pose[0] += delta_s * math.cos(theta_mid)
    pose[1] += delta_s * math.sin(theta_mid)
    pose[2] += delta_theta
    pose[2] = (pose[2] + math.pi) % (2 * math.pi) - math.pi

    return delta_s, delta_theta
  • it seems perfect, when we run only 1 lap (4 sides)

Pasted image 20250521171610.png

  • the odometry reports Pasted image 20250521171840.png

  • Suppose it should be x = 0; y = 0; θ = 360°

  • What caused the error?

  • What if we ran more laps?

  • 16 laps :
    Pasted image 20250522111354.png

  • the odometry reports:
    Pasted image 20250522111427.png

64 laps:
Pasted image 20250522111615.png

  • the odometry reports:
  • Pasted image 20250522111634.png

The problem comes from Angular Drift!!


Using Compass Sensor

  • Add a compass sensor to the robot.

  • Pasted image 20250522112326.png

  • Pasted image 20250522112420.png

  • Search for "compass"
    Pasted image 20250522112455.png

  • Getting compass values (Sample Code)

import math
from controller import Robot

compass = robot.getDevice("compass")
compass.enable(TIME_STEP)

def get_heading_angle(compass_values):
    rad = math.atan2(compass_values[0], compass_values[1])
    if rad < 0:
        rad += 2 * math.pi
    return rad

def compass_reading():
    compass_values = compass.getValues()
    current_heading = get_heading_angle(compass_values)
    print(f"Compass: {math.degrees(current_heading) % 360:.2f}")

Take note: the Webots simulator's compass has no noise, which never happens in the real world!

  • if we run 64 laps in the simulator with compass sensor.

Pasted image 20250522113532.png

  • the odometry reports:
    Pasted image 20250522114450.png

  • As for orientation:

  • Diff = (360° - 347.8° (from wheel encoder) ) + 0.85° (compass) = 13.05° degree

Let's add some noise to the compass sensor to make it more realistic

def get_heading_angle(compass_values):

    noise_std_deg = 2.0
    rad = math.atan2(compass_values[0], compass_values[1])

    if rad < 0:

        rad += 2 * math.pi

    noise = math.radians(random.gauss(0, noise_std_deg))
    noisy_heading = (rad + noise) % (2 * math.pi)

    return noisy_heading

Pasted image 20250522134032.png

It is what we likely to have in the real world

  • Now, we have two sensors and each of them has their strength and weakness.

  • Compass sensor (with noise):

    • Advantage: Error does not accumulate over time
    • Disadvantage: Insensitive to small orientation changes
  • Wheel encoder (for orientation):

    • Advantage: Sensitive to small orientation changes
    • Disadvantage: Error does accumulate over time

It is possible for us to combine them?


Complimentary Filter

A complementary filter is a simple and effective sensor fusion technique that combines two (or more) measurements that complement each other in the frequency domain—one sensor gives reliable high-frequency data, and the other gives reliable low-frequency data.

How It Works (In General)

The core idea is to blend the strengths of two sensors:

  • One sensor gives fast but noisy data (good at detecting rapid changes).
  • Another gives slow but stable data (good at long-term stability but poor at tracking fast changes).

Mathematically, a complementary filter for combining two signals A(t)A(t)A(t) and B(t)B(t)B(t) can be expressed as:

Where:

  • α is a tuning constant (e.g., between 0.90 to 0.98),
  • A(t) is typically the high-frequency sensor (e.g., gyro or encoder),
  • B(t) is typically the low-frequency sensor (e.g., compass or accelerometer).

Using Complementary Filter for Orientation with Compass and Wheel Encoder

  • Compass gives absolute heading (yaw) relative to magnetic north. It’s stable in the long term but noisy and affected by magnetic disturbances.
  • Wheel encoders give relative changes in orientation (by tracking differential wheel speeds), which are accurate short-term but drift over time due to wheel slip, uneven terrain, or integration errors.

Why It Works

  • Trust the wheel encoders for short-term changes (they quickly detect turns).
  • Trust the compass for long-term orientation (to correct drift).

The complementary filter might look like:

Where:

  • θencoder​: orientation change from encoders

  • θcompass: absolute heading from compass

  • “Trust the encoder for how things have changed.”

  • “Trust the compass for the long-term heading.”

  • Blend the two to get a more reliable heading.

Summary

  • Why it works: One sensor's weaknesses are offset by the other's strengths.
  • How it works: By weighting and blending sensor readings over time to produce a stable, accurate estimate.
  • In your case: Wheel encoders handle quick movements; compass provides slow, stable heading correction.

How do we tune the α?

  • if α is 0, then we trust the compass 100%, and encoder 0%.

  • The result of 16 laps.
    Pasted image 20250522204216.png

  • if α is 1.0, then we trust the compass 0%, and encoder 100%.

  • The result of 16 laps.

  • Pasted image 20250522204029.png

  • As mentioned earlier, α is a tuning constant (usaully between 0.90 to 0.98)

  • After some trial and error, we found that the best tuning constant for α is 0.94.

  • The result of 16 laps.
    Pasted image 20250522204450.png

  • The result looks promising in this case. However, it is not yet a real-world scenario, as the slippery model of the wheel has not been considered.


The Wheel Slippery Model

  • Adding dynamic noise to the wheel sensors
  • Assume that angular velocity is proportional to the slipperiness of the wheel.
  • Calculate the velocity
    # Compute velocities
def update_odometry(dt):
    '''
    '''
    linear_velocity = delta_s / dt
    angular_velocity = delta_theta_enc / dt
        dt = TIME_STEP / 1000.0  # Convert ms to seconds
        _, delta_theta = update_odometry(dt)
  • Add the function to inject the dynamic noise into wheel encoder readings
def add_wheel_noise(delta, velocity):

    # Add a velocity-correlated bias and Gaussian noise
    drift = 0.02 * velocity * random.uniform(-1, 1)  # dynamic bias
    noise = random.gauss(0, 0.05)  # random white noise
    return delta + drift + noise
  • let's see the outcome of 16 laps after adding the Wheel Slippery Model with respect to angular velocity.

  • it clearly shows that the complimentary cannot handle the dynamic noise.

  • particularly noise that violates the assumptions of complementary filters. e.g., low-frequency bias drift, correlated or state-dependent noise

  • Complementary filters assume high-frequency noise and no bias. They fail with slow drift or non-Gaussian dynamics like the velocity-correlated drift above.
    Pasted image 20250523102514.png

  • How to solve this problem? we need a better filtering algorithm! The Kalman filter!

Wrapping Up the Section

  1. Explain the concept of sensor fusion algorithm in robotics applications.
  2. Apply the complimentary algorithm for compass sensor + wheel encoder for mobile robot orientation.
  3. Describe the limitation of complimentary filter.